/**
 * @file DijkstraPlanner.cpp
 * @brief Dijkstra算法路径规划器实现
 */

#include "planners/DijkstraPlanner.h"
#include <queue>
#include <unordered_set>
#include <chrono>

namespace PathPlanning
{
DijkstraPlanner::DijkstraPlanner(std::shared_ptr<Map> map) : PlannerBase(map)
{
    // 设置默认配置
    config_ = std::make_shared<DijkstraConfig>();
}

PlanningResult DijkstraPlanner::plan(const Point& start, const Point& goal)
{
    PlanningResult result;
    auto           startTime = std::chrono::high_resolution_clock::now();

    // 验证起点和终点
    if (!validateStartGoal(start, goal))
    {
        result.message = "Invalid start or goal position";
        return result;
    }

    auto dijkstraConfig = std::static_pointer_cast<DijkstraConfig>(config_);

    // 初始化数据结构
    std::priority_queue<std::shared_ptr<DijkstraNode>, std::vector<std::shared_ptr<DijkstraNode>>, DijkstraNodeCompare>
        openSet;

    std::unordered_map<GridCell, std::shared_ptr<DijkstraNode>, GridCellHash> allNodes;
    std::unordered_set<GridCell, GridCellHash>                                closedSet;

    // 创建起点节点
    GridCell startCell = map_->worldToGrid(start);
    auto     startNode = std::make_shared<DijkstraNode>(startCell, start);
    startNode->gCost   = 0.0;
    startNode->fCost   = 0.0;

    openSet.push(startNode);
    allNodes[startCell] = startNode;

    GridCell goalCell = map_->worldToGrid(goal);

    int       iterations    = 0;
    const int maxIterations = 100000; // 防止无限循环

    while (!openSet.empty() && iterations < maxIterations)
    {
        iterations++;

        // 获取代价最小的节点
        auto currentNode = openSet.top();
        openSet.pop();

        GridCell currentCell = currentNode->gridCell;

        // 如果已经在关闭列表中，跳过
        if (closedSet.find(currentCell) != closedSet.end())
        {
            continue;
        }

        // 添加到关闭列表
        closedSet.insert(currentCell);
        result.exploredNodes.push_back(Pose(currentNode->position.x, currentNode->position.y, 0));

        // 检查是否到达目标
        if (currentCell.x == goalCell.x && currentCell.y == goalCell.y)
        {
            result.success = true;
            result.path    = reconstructPath(allNodes, currentNode);
            break;
        }

        // 定期调用可视化回调
        if (vizCallback_ && iterations % 1000 == 0)
        {
            result.iterations    = iterations;
            result.exploredNodes = result.exploredNodes; // 更新探索节点
            vizCallback_(result);
        }

        // 遍历邻居节点
        auto neighbors = getNeighbors(currentCell, dijkstraConfig->eightConnected);
        for (const auto& neighborCell : neighbors)
        {
            // 跳过障碍物和关闭列表中的节点
            if (map_->isObstacle(neighborCell.x, neighborCell.y) || closedSet.find(neighborCell) != closedSet.end())
            {
                continue;
            }

            Point  neighborWorld = map_->gridToWorld(neighborCell);
            double movementCost  = currentNode->gCost + euclideanDistance(currentNode->position, neighborWorld);

            // 检查是否是新节点或找到更优路径
            auto neighborIt = allNodes.find(neighborCell);
            if (neighborIt == allNodes.end())
            {
                // 新节点
                auto neighborNode    = std::make_shared<DijkstraNode>(neighborCell, neighborWorld);
                neighborNode->gCost  = movementCost;
                neighborNode->fCost  = movementCost; // Dijkstra中f=g
                neighborNode->parent = currentNode;

                openSet.push(neighborNode);
                allNodes[neighborCell] = neighborNode;
            }
            else
            {
                // 已有节点，检查是否需要更新
                auto neighborNode = neighborIt->second;
                if (movementCost < neighborNode->gCost)
                {
                    neighborNode->gCost  = movementCost;
                    neighborNode->fCost  = movementCost;
                    neighborNode->parent = currentNode;

                    // 需要重新加入优先队列以更新顺序
                    openSet.push(neighborNode);
                }
            }
        }
    }

    auto endTime        = std::chrono::high_resolution_clock::now();
    result.planningTime = std::chrono::duration<double>(endTime - startTime).count();
    result.iterations   = iterations;

    if (!result.success)
    {
        result.message = "Path not found within iteration limit";
    }
    else
    {
        // 计算路径长度
        for (size_t i = 1; i < result.path.size(); ++i)
        {
            result.pathLength += euclideanDistance(result.path[i - 1].toPoint(), result.path[i].toPoint());
        }
        result.message = "Path found successfully";
    }

    return result;
}

std::vector<Pose> DijkstraPlanner::reconstructPath(
    const std::unordered_map<GridCell, std::shared_ptr<DijkstraNode>, GridCellHash>& allNodes,
    const std::shared_ptr<DijkstraNode>&                                             endNode)
{
    std::vector<Pose> path;
    auto              currentNode = endNode;

    while (currentNode != nullptr)
    {
        path.push_back(Pose(currentNode->position.x, currentNode->position.y, 0));
        if (currentNode->parent)
        {
            currentNode = std::static_pointer_cast<DijkstraNode>(currentNode->parent);
        }
        else
        {
            break;
        }
    }

    std::reverse(path.begin(), path.end());
    return smoothPath(path);
}

} // namespace PathPlanning